/*
  countBlips - measures the time taken to move some 
  number of tachometer "blips"
  
  Modified for two motors
  */
  
int ledPin11 = 11;                      // Motor1 LED (pin 11)
int ledPin12 = 12;                      // Motor2 LED (pin 12) 

volatile unsigned long blipCounter1 = 0;  // Counter for number of blips seen on Tach1 (from motor1)
volatile unsigned long blipCounter2 = 0;  // Counter for number of blips seen on Tach2 (from motor2)

volatile int toggleTrigger1 = LOW;        // Toggle this var whenever set number of counts is reached on blipCounter1
volatile int toggleTrigger2 = LOW;        // Toggle this var whenever set number of counts is reached on blipCounter2
int toggleTrigger1_last = LOW;            // Last toggle capture from counter1
int toggleTrigger2_last = LOW;            // Last toggle capture from counter2

unsigned long current_time_meas1 = 0;    // Variable to store current time in microseconds for counter1
unsigned long last_time_meas1    = 0;       // Variable to store last time in microseconds for counter1
unsigned long elapsed_time1      = 0;         // The time taken for Tach1 to rotate the set number of blips
unsigned long current_time_meas2 = 0;    // Variable to store current time in microseconds for counter2
unsigned long last_time_meas2    = 0;       // Variable to store last time in microseconds for counter2
unsigned long elapsed_time2      = 0;         // The time taken for Tach2 to rotate the set number of blips

int setNumBlips = 500;                   // Measure time tach takes to move this number of blips

char outputBuffer[256];

void setup()
{
  pinMode(ledPin11, OUTPUT);              // Configure motor1 LED pin
  pinMode(ledPin12, OUTPUT);              // Configure motor2 LED pin
  digitalWrite(2, HIGH);                 // Set pullup resistor on external interrupt pin 2 (data from Tach1)
  digitalWrite(3, HIGH);                 // Set pullup resistor on external interrupt pin 3 (data from Tach2)
  
  attachInterrupt(0, countBlips1, RISING);  // Attach Interrupt Routine to External Interrupt 0, on pin 2, to trigger an interrupt on signal's rising edge (Tach1)
  attachInterrupt(1, countBlips2, RISING);  // Attach Interrupt Routine to External Interrupt 1, on pin 3, to trigger an interrupt on signal's rising edge (Tach2)
  
  Serial.begin(9600);                    // Start serial connection
}

void loop()
{                                                      // Test Tach1
  if (toggleTrigger1 != toggleTrigger1_last)           // Every time this is true, the tacho has moved 500 blips
  {                                                    // If I measure the time between these occurences, I can estimate motor speed                                        
    last_time_meas1 = current_time_meas1;              // Change current time measured to last time measured 
    current_time_meas1 = micros();                     // Measure time of current occurance (the occurrence is that toggleTrigger has changed state
    elapsed_time1 = current_time_meas1 - last_time_meas1; // Calculate elapsed time
    digitalWrite(ledPin11, toggleTrigger1);             // Blink LED for visual confirmation 
    toggleTrigger1_last = toggleTrigger1 ;             // Save current toggle state to last toggle state  
    // Just added this Thurs Sept 15 to see what ser port is doing
    //Serial.println("Elapsed_time1 is: " + elapsed_time1);
    //Serial.println("\n");
    //sprintf(outputBuffer, "Elapsed: %l", (double)elapsed_time1);
    //Serial.println(outputBuffer);
    //Serial.println(elapsed_time1);
    Serial.print("Elapsed_time1 is : ");
    Serial.println(elapsed_time1);
  }
   if (toggleTrigger2 != toggleTrigger2_last)          // Test Tach2u
  {                                                                                       
    last_time_meas2 = current_time_meas2;              
    current_time_meas2 = micros();                    
    elapsed_time2 = current_time_meas2 - last_time_meas2; 
    digitalWrite(ledPin12, toggleTrigger2);            
    toggleTrigger2_last = toggleTrigger2 ; 
    
   // Serial.println("Elapsed_time2 is: " + elapsed_time2);    
   Serial.print("Elapsed_time2 is : ");
    Serial.println(elapsed_time2);
  
  }
  
    //Serial.println( elapsed_time1 + "|" elapsed_time2 );    // Send times to serial port 
  
}

void countBlips1()          // Routine to run upon recieving interrupt on pin 2 (from Tach1)
{
  blipCounter1++;           // Just increment the blip counter1
  if (blipCounter1 == setNumBlips)      // Check if 500 counts yet
    {
      toggleTrigger1 = !toggleTrigger1; // Change trigger state
      blipCounter1 = 0;                 // Clear counter
    }
}

void countBlips2()          // Routine to run upon recieving interrupt on pin 3 (from Tach2)
{
  blipCounter2++;           // Just increment the blip counter
  if (blipCounter2 == setNumBlips)      // Check if 500 counts yet
    {
      toggleTrigger2 = !toggleTrigger2; // Change trigger state
      blipCounter2 = 0;                 // Clear counter
    }
}
